#include "TargetFeeder.hpp"
#include "MatWork.hpp"



void TargetFeeder::on_mouse( int event, int x, int y, int flags, void* param )
{
	//	if( !frame )
	//		return;

	uchar ch1,ch2, ch3;	
	Point3i sample; 	


	switch( event )
	{
	case CV_EVENT_LBUTTONDOWN:

		sample.x = ch1;
		sample.y = ch2;
		sample.z = ch3;

		printf("click point is (%d,%d) -> (R,G,B) = (%d,%d,%d)\n",x,y, sample.x, sample.y, sample.z);

		clicked_points.push_back(Point2i(x, y));

		//writeTextFile();

		break;

	case CV_EVENT_LBUTTONUP:
		break;
	}
}


TargetFeeder::TargetFeeder(Mat& InputImage, Mat& InputObstacles)
{
	// get map - array of points & an image
	InputImage.copyTo(map_image);
}


// visualize the map and use mouse click to define the workspace
void TargetFeeder::visualize()
{
	char* wind_name = "Choose Workspace";
	char c;

	// draw obstacles on image
	drawCircles(map_image, map_obstacles);

	imshow(wind_name, map_image);

	setMouseCallback(wind_name, on_mouse, 0);   // bug is here! cannot use mouseCallback

	// click 2 corners of workspace
	while (clicked_points.size() < 2)
	{
		printf("Please click 2 POINTS to create workspace!!");

		c = cvWaitKey(1);
		if (c == 27) 
		{
			break;
		}
	}
};


// generate random targets with normal distribution
void TargetFeeder::generate(int num_point)
{
	Mat via_pnts_col = Mat::zeros(1, num_point, CV_16U);
	Mat via_pnts_row = Mat::zeros(1, num_point, CV_16U);

	RNG().fill(via_pnts_col, RNG::UNIFORM, clicked_points[0].x, clicked_points[1].x);
	RNG().fill(via_pnts_row, RNG::UNIFORM, clicked_points[0].y, clicked_points[1].y);

	Point p;
	for (int i=0; i<num_point; i++)
	{
		p.x = via_pnts_col.at<uint>(i);
		p.y = via_pnts_row.at<uint>(i);

		via_points.push_back(p);
	}

	// feed to return trajectory
	trajectory = Mat(via_points, true);
};


// create path through all points - Dijsktra or not :))

TargetFeeder::~TargetFeeder(void)
{

}











